#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"

void testTime(int duration);
void testEncoders(int endValue);

int main(void){
	int motorDuration=100;
	int encoderDuration=100;
	LockoutProtection();
	InitializeMCU();

	testTime(motorDuration);
	//testEncoders(encoderDuration);
}

/**
 *This method tests to see how far the robot goes per unit of time
 *Result: 
 */
void testTime(int duration){
	int maxSpeed=127;
	InitializeMotors(false, false);

	SetMotorPowers(maxSpeed, maxSpeed);
	Wait(duration);

	SetMotorPowers(0,0);
}

/**
 *This method tests to see how far the robot goes per encoder value
 *Result: 
 */
void testEncoders(int endValue){
	int maxSpeed=127;
	InitializeEncoders(false, false);
	signed long encoder0, encoder1, counter = 0;
	PresetEncoderCounts(0, 0);

	SetMotorPowers(maxSpeed, maxSpeed);
	while(1){
		encoder0 = GetEncoderCount(ENCODER_0);	 
		encoder1 = GetEncoderCount(ENCODER_1);
		GetEncoderCounts(&encoder0, &encoder1);
		if((encoder0+encoder1)/2>=endValue)break;
	}

	SetMotorPowers(0,0);
}